/**
  **********************************************************************************
  * @file   : Service_Debug.cpp
  * @brief  : Debug support file.This file provides access ports to debug.
  **********************************************************************************
  *  
**/
/* Includes ------------------------------------------------------------------*/
#include "Service_Debug.h"
#include "Service_Communication.h"
#include "Serial_Plot.h"

/* Private define ------------------------------------------------------------*/
TaskHandle_t Debug_Handle;
/* Private variables ---------------------------------------------------------*/
/* Private function declarations ---------------------------------------------*/
void Task_Debug(void *arg);

/* Function prototypes -------------------------------------------------------*/
/**
* @brief  Initialize debug service based on Upper Monitor.
*/
void Service_Debug_Init(void)
{
    xTaskCreate(Task_Debug,          /* Task function. */
                "Debug_Service",     /* Task Name. */
                Normal_Stack_Size,   /* Stack depth. */
                NULL,                /* Task parameter */
                PriorityBelowNormal, /* Priority */
                &Debug_Handle);      /* Task handle */
}


// void Variable_Send_Packet(void)
// {
//     serial_plot.Add_Var(DR16.Get_LX_Norm());
//     serial_plot.Add_Var(DR16.Get_LY_Norm());
//     serial_plot.Add_Var(DR16.Get_RX_Norm());
//     serial_plot.Add_Var(DR16.Get_RY_Norm());
//     serial_plot.Add_Var(DR16.GetSW_L());
//     serial_plot.Add_Var(DR16.Get_Yaw_Norm());
//     serial_plot.Add_Var(DR16.GetStatus());
// }
// Serial_Plot DR16Data(NULL, 0, NULL, Variable_Send_Packet);

/**
* @brief  Send the debug meaasge to Upper Monitor(Lowest priority)
* @param  None.
* @return None.
*/
void Task_Debug(void *arg)
{
    for (;;)
    {
        static uint16_t prescale=0;
        if(prescale++>=10)
        {
            prescale = 0;
            // DR16Data.Send_Packet();
            
        }
        
        vTaskDelay(1);  // Pass to next ready task
    }
}

/************************ COPYRIGHT(C) USTC-ROBOWALKER **************************/
